# -*- coding: utf-8 -*-
"""
Created on Fri Oct  2 15:48:22 2020

@author: dalaska
"""
#Coordinate rotation and translate

import cv2
import matplotlib.pyplot as plt
import matplotlib as mpl
import numpy as np
import math
import copy 
class point2D():
    def __init__(self):
        self.pos = np.array([[0],[0],[1]])       
        self.color = 'r'
    def where(self):
        print(self.pos)
    def setpos(self,pos): # set position   
        self.pos[0] = pos[0]
        self.pos[1] = pos[1]
    def show(self):
        print(self.pos)
    def rotate(self,deg):
        rad = np.deg2rad(deg)
        R = np.array([ [ math.cos(rad),  math.sin(rad),  0],
                       [-math.sin(rad),  math.cos(rad),  0],
                       [             0,              0,  1]]);
        self.pos = R.dot(self.pos)
    def translate(self,x): # x is a vector
        self.pos[0] = self.pos[0]+x[0]
        self.pos[1] = self.pos[1]+x[1]
    def transhomo(self,x):
        #Homogeneous coordinates
        T = np.array([ [1,  0,  x[0]],
                       [0,  1,  x[1]],
                       [0,  0,    1]]);
        self.pos = T.dot(self.pos)
        
        
        
# create new
pt0 = point2D()
pt0.setpos([0,2])
pt0.where()

pt1 = copy.deepcopy(pt0) # use deepcopy, don't use = 
pt1.rotate(90) # rotate 30 deg
pt1.translate([0,5]) 

pt2 = copy.deepcopy(pt0) 
pt2.translate([0,5]) 
pt2.rotate(90) 
# rotate and translate is not commutative

pt3 = copy.deepcopy(pt0) 
pt3.rotate(90) 
pt3.rotate(-90) 
# rotate is commutative, r is symmetric mat

pt4 = copy.deepcopy(pt0) 
pt4.transhomo([0,5]) 




# show
fig = plt.figure(1)          #添加一个窗口
ax = fig.add_subplot(1,1,1)   #在窗口上添加一个子图
plt.xlim(-20, 20)
plt.ylim(-20, 20)
# mpl.axes.Axes.set_aspect( aspect = 'equal', adjustable=None, anchor=None, share=False)
ax.set_aspect('equal')
ax.scatter(0,0,c='y',marker='x',label = 'center') 
ax.scatter(pt0.pos[0],pt0.pos[1],c='r',label = 'pt0') 
ax.scatter(pt1.pos[0],pt1.pos[1],c='b',label = 'pt1')  
ax.scatter(pt2.pos[0],pt2.pos[1],c='g',label = 'pt2')  
ax.scatter(pt3.pos[0],pt3.pos[1],c='m',label = 'pt3')  
ax.scatter(pt4.pos[0],pt4.pos[1],c='c',label = 'pt4')  

ax.legend()
plt.grid()
# plt.savefig('foo.jpg')
plt.show()  #所有窗口运行
    
if 0: # temp
# 3d scatter
# https://matplotlib.org/3.1.3/gallery/mplot3d/scatter3d.html
    t1 = np.array([[1,  0,  3],
                  [0,  1,  1],
                  [3,  1,  1]]);
    
    
    t2 = np.array([[1,  2,  0],
                  [2,  1,  0],
                  [0,  0,  1]]);
    
    t3 = t1.dot(t2)
    t4 = t2.dot(t1)